#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <std_msgs/String.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sstream>
#include <demo_video_processing/pointinfo.h>
#include <demo_video_processing/pertrans.h>
#include <demo_video_processing/Transform.h>
using namespace std;
using namespace cv;
static const std::string OPENCV_WINDOW = "Raw Image window";
static const std::string OPENCV_WINDOW_1 = "Edge Detection";
/******************************************************************************************************/

Mat curImg, preImg1, preImg2;
Mat curimgThresholded;
Mat diff1_2, diff2_3, diff, binImg,diff_color_sport;
Mat mask;
Mat src_for_transform;
Mat src_for_color;
int i=1;
int ct_size;
Mat srcimage;
Mat src_for_preinit;
vector<Point2d> box_point;//目标质心坐标集合
vector<Point2f> realPoints;//从opti订阅的真实车辆坐标
codiConvertor* codiConverter =new codiConvertor();//codiConvertor类对象定义
vector<point4Op> cornPoints;//存储烧饼轨道角点
vector<Point2f> recgPoints;//梯度下降：存储原图中识别到的车辆坐标
vector<Point2f> calcPoints;//梯度下降：由recgPoints透射变换所得计算坐标
int iLowH = 111;
int iHighH = 162;

int iLowS = 192; 
int iHighS = 255;

int iLowV = 134;
int iHighV = 255;
/*
Mat frame,previousframe,currentframe,s3,difframe,difframe2,difframe3,tempframe;
void detect_edges(cv::Mat img)
{

    cv::Mat src, src_gray;
    cv::Mat dst, detected_edges;

    int edgeThresh = 1;
    int lowThreshold = 200;
    int highThreshold =300;
    int kernel_size = 5;

    img.copyTo(src);

    cv::cvtColor( img, src_gray, CV_BGR2GRAY );
    cv::blur( src_gray, detected_edges, cv::Size(5,5) );
    cv::Canny( detected_edges, detected_edges, lowThreshold, highThreshold, kernel_size );

    dst = cv::Scalar::all(0);
    img.copyTo( dst, detected_edges);
    dst.copyTo(img);

    imshow(OPENCV_WINDOW, src);
    imshow(OPENCV_WINDOW_1, dst);
    cv::waitKey(1);

}	
*/
vector<Point2d> draw_rect(Mat srcImg_bgr,Mat srcImg_bin, Mat dstImg)
{
	vector<vector<Point>> contours;//保存轮廓
	vector<Vec4i> hierarcy;//保存轮廓层次信息
	findContours(srcImg_bin, contours, hierarcy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);//轮廓检测
	vector<Rect> boundRect(contours.size());  //定义外接矩形集合
	vector<RotatedRect> box(contours.size()); //定义最小外接矩形集合
	vector<Point2d> box_point;
    Point2f rect[4];//保存矩形的四个顶点，用浮点型定义
	for (int i = 0; i < contours.size(); i++)
	{
		box[i] = minAreaRect(Mat(contours[i]));  //计算每个轮廓最小外接矩形
		boundRect[i] = boundingRect(Mat(contours[i]));
		circle(dstImg, Point(box[i].center.x, box[i].center.y), 5, Scalar(0, 255, 0), -1, 8);  //绘制最小外接矩形的中心点
		// cout<<box[i].center<<endl;
        
        box[i].points(rect);  //把最小外接矩形四个端点复制给rect数组
		if(boundRect[i].area()>300)
        {
            char tam[100];
            sprintf(tam,"%0.0f,%0.0f",box[i].center.x,box[i].center.y);//字符串格式化
            circle(srcImg_bgr, Point(box[i].center.x, box[i].center.y), 5, Scalar(0, 0, 255), -1, 8);  //绘制最小外接矩形的中心点
            rectangle(srcImg_bgr, Point(boundRect[i].x, boundRect[i].y), Point(boundRect[i].x + boundRect[i].width, boundRect[i].y + boundRect[i].height), Scalar(0, 0, 255), 2, 8);
            putText(srcImg_bgr,tam,Point(box[i].center.x-8,box[i].center.y+8),FONT_HERSHEY_SIMPLEX,1.0,Scalar(255,255,255),2);
            box_point.push_back(Point2d(box[i].center.x, box[i].center.y));
        }
	}
    ct_size=contours.size();
    waitKey(1);
    return box_point;
}
 void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
    try
    {
        curImg = cv_bridge::toCvCopy(msg, "bgr8")->image;
        curImg.copyTo(srcimage);
        curImg.copyTo(src_for_transform);
        curImg.copyTo(src_for_color);
        curImg.copyTo(src_for_preinit);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert image1 from '%s' to 'bgr8'. ", msg->encoding.c_str());
    }
    if(!curImg.empty())
    {
        cvtColor(curImg, curImg, CV_BGR2GRAY);
    }
    if(!preImg2.empty()&&!curImg.empty())
    {
/****************************3帧差法实现运动物体过滤***************************/

        absdiff(curImg, preImg1, diff1_2);//做差求绝对值   1-2
        absdiff(preImg1, preImg2, diff2_3);//做差求绝对值            2-3
        bitwise_and(diff1_2, diff2_3, diff);
        add(diff1_2, diff2_3, diff);
        threshold(diff, binImg, 30, 255.0, CV_THRESH_BINARY);//二值化，将大于30的点赋值为255
        if(!mask.empty())bitwise_and(binImg,mask, binImg);
        /***************以下这一部分是只进行帧差法识别时进行的形态学操作轮廓提取
                                     ，若采用颜色和运动双重识别，则应将这一部分注释掉**********/
        imshow("binImg",binImg);
        Mat kerErd=getStructuringElement(MORPH_RECT,Size(2,2));
        Mat kerDil=getStructuringElement(MORPH_RECT,Size(16,16));
        // dilate(binImg, binImg, kerDil);//膨胀
        erode(binImg, binImg, kerErd);//腐蚀
        // blur(binImg,binImg,Size(5,5));
        dilate(binImg, binImg, kerDil);
        waitKey(1);
        vector<Point2d> box_point =draw_rect(srcimage,binImg,binImg);//执行识别运动车辆函数




/*********************颜色识别阈值调试轨迹条（调阈值时使用）*****************************/
        //  namedWindow("Control", CV_WINDOW_AUTOSIZE); //create a window called "Control"
        // cvCreateTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
        // cvCreateTrackbar("HighH", "Control", &iHighH, 179);

        // cvCreateTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
        // cvCreateTrackbar("HighS", "Control", &iHighS, 255);

        // cvCreateTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
        // cvCreateTrackbar("HighV", "Control", &iHighV, 255);

/********************************颜色识别*****************************************/
//    Mat imgHSV;
//    vector<Mat> hsvSplit;
//    cvtColor(src_for_color, imgHSV, COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV
//    //因为我们读取的是彩色图，直方图均衡化需要在HSV空间做
//    split(imgHSV, hsvSplit);
//    equalizeHist(hsvSplit[2],hsvSplit[2]);
//    merge(hsvSplit,imgHSV);
//     inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), curimgThresholded); //Threshold the image


/********************颜色识别与运动识别取交********************/
    // bitwise_and(curimgThresholded,binImg,diff_color_sport);
    // Mat element = getStructuringElement(MORPH_RECT, Size(13, 13));
    // dilate(diff_color_sport, diff_color_sport, element);//膨胀
/*****************************绘出交集图像轮廓****************************/
    // box_point=draw_rect(srcimage,diff_color_sport,diff_color_sport);
    // imshow("diff_color_sport",srcimage);
    // waitKey(10);
/************************************************************************/

/********************************形态学操作（一般不做）*****************************/
//    //开操作 (去除一些噪点)
   
//    morphologyEx(curimgThresholded, curimgThresholded, MORPH_CLOSE, element);
    // dilate(curimgThresholded, curimgThresholded, element);//膨胀
    // imshow("dilate", curimgThresholded); //show the thresholded image
//    imshow("Thresholded Image", curimgThresholded); //show the thresholded image
//    imshow("src_for_color", src_for_color); //show the original image
/************************************************************************/

/****************实现原始坐标向俯视坐标转换*********************/
    float ok = TransformInit(src_for_transform);                                  //
	Mat tp = GetTranMatrix(), tt;                                          //初
	invert(tp, tt);                                                                        //
	Mat trans_dst;                                                                     //
    warpPerspective(                                                              //始
    src_for_transform,                                                                                     //
    trans_dst,                                                                              //
    tt,                                                                                              //化
    cv::Size(1500, 2714),                                                         //
    cv::INTER_LINEAR | cv::WARP_INVERSE_MAP);    //




/*********************************************************************************/
    codiConverter->mapMartix=tt;//将计算所得透射矩阵赋给梯度下降类的矩阵
/*********************************************************************************/



	 vector<Point2d> t;       //存储检测到的车辆坐标
   for(auto box_pause :box_point)
   {
        t.push_back(Point2d(box_pause.x,box_pause.y));
   }
    vector<Point3d> r = camera2world(t);//执行坐标变换，r用来存储变换后的俯视坐标
    i=0;
    for(auto &r_pause : r)//在透视变换图中圈出车辆，打印z坐标
    {
        circle(trans_dst,Point2d(r_pause.x,r_pause.y),20,Scalar(0,255,0),-1,8);
        char world_coordi[20];
        sprintf(world_coordi,"%0.0f,%0.0f,%0.0f",r_pause.x,r_pause.y,r_pause.z);
        putText(trans_dst,world_coordi,Point(r_pause.x-10,r_pause.y-10),FONT_HERSHEY_SIMPLEX,3.0,Scalar(0,255,255),4);
 /*********************************************************************************/
        calcPoints.push_back(Point2f(r_pause.x,r_pause.y));//透射变换后坐标赋给梯度下降所用计算坐标
/*********************************************************************************/
    }
namedWindow("trans_dst",0);
cvResizeWindow("trans_dst",750,1357);
imshow("trans_dst",trans_dst);//显示透视变换图

    /***************************************梯度下降****************************************/
        // Mat res;
		// warpPerspective(src_for_preinit, res, trans, Size(640, 480), 1, BORDER_REPLICATE);
		// imshow(" ", res);
		// waitKey(5);
        if(!realPoints.empty())//真实坐标不空才能玩梯度下降
        {
                vector<point4Op> opPoints;
                point4Op curcar;
                float mindis;
                for (auto& calcPoint : calcPoints)
                {
                    mindis = 10000000;
                    for (auto& realPoint : realPoints)
                    {
                        if (distance(calcPoint, realPoint) < mindis)
                        {
                            curcar.realpoint = realPoint;
                            mindis = distance(calcPoint, realPoint);
                        }
                    }
                    curcar.calcpoint = calcPoint;
                    opPoints.push_back(curcar);
                }
                codiConverter->points4Op = opPoints;
                codiConverter->optimizer(0.000001);
        }
         /*******************************************************************************/
    }
    preImg2=preImg1;
    preImg1=curImg;
}

void imageCb2(const sensor_msgs::ImageConstPtr& msg1)
{
        try
        {
            mask = cv_bridge::toCvCopy(msg1, "bgr8")->image;
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("Could not convert image1 from '%s' to 'bgr8'. ", msg1->encoding.c_str());
        }
        if(!mask.empty())
        {
            cvtColor(mask, mask, CV_BGR2GRAY);
            threshold(mask, mask, 30, 255.0, CV_THRESH_BINARY);//二值化，将大于15的点赋值为255
            cv::resize(mask,mask,Size(1280,720));
        }
}

void opti_callback(const demo_video_processing::pointinfo::ConstPtr &msg)
{
    try
    {
                for(int ai=0;ai<msg->number;ai++)
        {
            realPoints[ai].x=msg->points[ai].xaxis;
            realPoints[ai].y=msg->points[ai].yaxis;
        }
        // if(!realPoints.empty())cout<<realPoints[0].x;
        if(realPoints.empty())
        {
            throw -1;
        }
    }
    catch(int e)
    {
        cout<<"妹收到opti消息！"<<endl;
    }
}

int main(int argc, char** argv)
{
    /****************pub&sub init**************/
    ros::init(argc, argv, "image_subscribe");
    ros::NodeHandle nh;

    
    demo_video_processing::point point_msg;
    ros::Publisher point_pub =nh.advertise< demo_video_processing::pointinfo>("axis",30);

    // ros::Subscriber sub2 = nh.subscribe("mask",1, imageCb2);
    ros::Subscriber sub1 = nh.subscribe("/camera/image_raw",5, imageCb);
    ros::Subscriber opti_sub=nh.subscribe("opti",10,opti_callback);
    ros::Rate loop_rate1(10);
    while(ros::ok())
{
        /*******************msg processing & sub callback******************/
    int Ai=0;
    demo_video_processing::pointinfo msg;
    for(auto &axis : box_point)
    {
        point_msg.id=Ai+1;
        point_msg.xaxis=axis.x;
        point_msg.yaxis=axis.y;
        point_msg.high=0;
        msg.points.push_back(point_msg);
        Ai++;
    }
    msg.number=Ai;
    point_pub.publish(msg);
    ros::spinOnce();
    loop_rate1.sleep();
}
  return 0;
}